#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j =  (((p.last_leaf() / 3) * 3 - 3) / 12) * 3;  
  while (i < j) {
    if(less(*(a + i + 1), *(a + i + 2))) {
      *(a + i) = *(a + i + 2);
      if(less(*(a + (i << 2) + 12), *(a + (i << 2) + 15))) {
	if(less(*(a + (i << 2) + 15), x)) {
	  if(less(*(a + i), x))
	    *(a + i) = x;
	  else 
	    *(a + i + 2) = x;
	  return;
	}
	*(a + i + 2) = *(a + (i << 2) + 15);
	i = (i << 2) + 15;
      }
      else {
	if(less(*(a + (i << 2) + 12), x)) {
	  if(less(*(a + i), x))
	    *(a + i) = x;
	  else 
	    *(a + i + 2) = x;
	  return;
	}
	*(a + i + 2) = *(a + (i << 2) + 12);
	i =  (i << 2) + 12;
      }
    }
    else {
      *(a + i) = *(a + i + 1);
      if(less(*(a + (i << 2) + 6), *(a + i * 4 + 9))) {
	if(less(*(a + (i << 2) + 9), x)) {
	  if(less(*(a + i), x))
	    *(a + i) = x;
	  else 
	    *(a + i + 1) = x;
	  return;
	}
	*(a + i + 1) = *(a + (i << 2) + 9);
	i = (i << 2) + 9;
      }
      else {
	if(less(*(a + (i << 2) + 6), x)) {
	  if(less(*(a + i), x))
	    *(a + i) = x;
	  else 
	    *(a + i + 1) = x;
	  return;
	}
	*(a + i + 1) = *(a + (i << 2) + 6);
	i = (i << 2) + 6;
      }
    }
  }
  
 while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, a[j])) {
      p.update_all_present(a, i, a[j], ordering());
    }
    else {
      p.update_all_present(a, i, x, ordering());
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, ordering());	
    else 
      j = p.top_some_absent(a, i, ordering());
    if (less(x, a[j])) {
      p.update_all_present(a, i, a[j], ordering());
      i = j;
    }
    else {
      p.update_all_present(a, i, x, ordering());
      return;
    }
  }    
  p.update_all_present(a, i, x, ordering());
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j =  (((p.last_leaf() / 3) * 3 - 3) / 12) * 3;  

 while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, a[j])) {
      p.update_all_present(a, i, a[j], ordering());
    }
    else {
      p.update_all_present(a, i, x, ordering());
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, ordering());	
    else 
      j = p.top_some_absent(a, i, ordering());
    if (less(x, a[j])) {
      p.update_all_present(a, i, a[j], ordering());
      i = j;
    }
    else {
      p.update_all_present(a, i, x, ordering());
      return;
    }
  }    
  p.update_all_present(a, i, x, ordering());
  return;   

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


